﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace FaceRecognition
{
    class Matrix
    {

        // --------------------------------------------------------------------------------------------------------------
        public static double[,] MatrixCreate(int rows, int cols)
        {
            // allocates/creates a matrix initialized to all 0.0. assume rows and cols > 0
            // do error checking here
            double[,] result = new double[rows, cols];
            return result;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[,] MatrixDecompose(double[,] matrix, out int[] perm, out int toggle)
        {
            // Doolittle LUP decomposition with partial pivoting.
            // rerturns: result is L (with 1s on diagonal) and U; perm holds row permutations; toggle is +1 or -1 (even or odd)
            int rows = matrix.GetLength(0);
            int cols = matrix.GetLength(1);

            //Check if matrix is square
            if (rows != cols)
                throw new Exception("Attempt to MatrixDecompose a non-square mattrix");

            double[,] result = MatrixDuplicate(matrix); // make a copy of the input matrix

            perm = new int[rows]; // set up row permutation result
            for (int i = 0; i < rows; ++i) { perm[i] = i; } // i are rows counter

            toggle = 1; // toggle tracks row swaps. +1 -> even, -1 -> odd. used by MatrixDeterminant

            for (int j = 0; j < rows - 1; ++j) // each column, j is counter for coulmns
            {
                double colMax = Math.Abs(result[j, j]); // find largest value in col j
                int pRow = j;
                for (int i = j + 1; i < rows; ++i)
                {
                    if (result[i, j] > colMax)
                    {
                        colMax = result[i, j];
                        pRow = i;
                    }
                }

                if (pRow != j) // if largest value not on pivot, swap rows
                {
                    double[] rowPtr = new double[result.GetLength(1)];

                    //in order to preserve value of j new variable k for counter is declared
                    //rowPtr[] is a 1D array that contains all the elements on a single row of the matrix
                    //there has to be a loop over the columns to transfer the values
                    //from the 2D array to the 1D rowPtr array.
                    //----tranfer 2D array to 1D array BEGIN

                    for (int k = 0; k < result.GetLength(1); k++)
                    {
                        rowPtr[k] = result[pRow, k];
                    }

                    for (int k = 0; k < result.GetLength(1); k++)
                    {
                        result[pRow, k] = result[j, k];
                    }

                    for (int k = 0; k < result.GetLength(1); k++)
                    {
                        result[j, k] = rowPtr[k];
                    }

                    //----tranfer 2D array to 1D array END

                    int tmp = perm[pRow]; // and swap perm info
                    perm[pRow] = perm[j];
                    perm[j] = tmp;

                    toggle = -toggle; // adjust the row-swap toggle
                }

                if (Math.Abs(result[j, j]) < 1.0E-20) // if diagonal after swap is zero . . .
                    return null; // consider a throw

                for (int i = j + 1; i < rows; ++i)
                {
                    result[i, j] /= result[j, j];
                    for (int k = j + 1; k < rows; ++k)
                    {
                        result[i, k] -= result[i, j] * result[j, k];
                    }
                }
            } // main j column loop

            return result;
        } // MatrixDecompose

        // --------------------------------------------------------------------------------------------------------------
        public static double MatrixDeterminant(double[,] matrix)
        {
            int[] perm;
            int toggle;
            double[,] lum = MatrixDecompose(matrix, out perm, out toggle);
            if (lum == null)
                throw new Exception("Unable to compute MatrixDeterminant");
            double result = toggle;
            for (int i = 0; i < lum.GetLength(0); ++i)
                result *= lum[i, i];

            return result;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[,] MatrixDuplicate(double[,] matrix)
        {
            // allocates/creates a duplicate of a matrix. assumes matrix is not null.
            double[,] result = MatrixCreate(matrix.GetLength(0), matrix.GetLength(1));
            for (int i = 0; i < matrix.GetLength(0); ++i) // copy the values
                for (int j = 0; j < matrix.GetLength(1); ++j)
                    result[i, j] = matrix[i, j];
            return result;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[,] ExtractLower(double[,] matrix)
        {
            // lower part of a Doolittle decomposition (1.0s on diagonal, 0.0s in upper)
            int rows = matrix.GetLength(0); int cols = matrix.GetLength(1);
            double[,] result = MatrixCreate(rows, cols);
            for (int i = 0; i < rows; ++i)
            {
                for (int j = 0; j < cols; ++j)
                {
                    if (i == j)
                        result[i, j] = 1.0f;
                    else if (i > j)
                        result[i, j] = matrix[i, j];
                }
            }
            return result;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[,] ExtractUpper(double[,] matrix)
        {
            // upper part of a Doolittle decomposition (0.0s in the strictly lower part)
            int rows = matrix.GetLength(0); int cols = matrix.GetLength(1);
            double[,] result = MatrixCreate(rows, cols);
            for (int i = 0; i < rows; ++i)
            {
                for (int j = 0; j < cols; ++j)
                {
                    if (i <= j)
                        result[i, j] = matrix[i, j];
                }
            }
            return result;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[,] MatixTranspose(double[,] matrix)
        {
            int nrows = matrix.GetLength(0);
            int ncols = matrix.GetLength(1);

            double[,] transposedMatrix = MatrixCreate(matrix.GetLength(1), matrix.GetLength(0));
            for (int i = 0; i < nrows; i++)
            {
                for (int j = 0; j < ncols; j++)
                {
                    transposedMatrix[j, i] = matrix[i, j];
                }
            }
            return transposedMatrix;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[] MatrixVectorMultiply(double[,] A, double[] x)
        {
            int m = A.GetLength(0);
            int n = A.GetLength(1);


            if (x.Length != n)
            {
                Console.WriteLine("Illegal matrix dimensions.");
                return null;
            }

            double[] y = new double[m];

            for (int i = 0; i < m; i++)
                for (int j = 0; j < n; j++)
                    y[i] += (A[i, j] * x[j]);
            return y;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static void MatrixZeros(double[,] A)
        {
            for (int i = 0; i < A.GetLength(0); i++)
            {
                for (int j = 0; j < A.GetLength(1); j++)
                {
                    A[i, j] = 0;
                }
            }
        }

        // --------------------------------------------------------------------------------------------------------------
        public static void VectorZeros(double[] A)
        {
            for (int i = 0; i < A.GetLength(0); i++)
            {
                A[i] = 0;
            }
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double Dot(double[] x, double[] y)
        {
            if (x.Length != y.Length)
            {
                Console.WriteLine("Illegal vector dimensions.");
                return 0;
            }

            double sum = 0.0;
            for (int i = 0; i < x.Length; i++)
                sum += x[i] * y[i];
            return sum;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[] MatrixGetColumn(double[,] A, int column)
        {
            double[] temp = new double[A.GetLength(0)];
            for (int i = 0; i < A.GetLength(0); i++)
            {
                temp[i] = A[i, column];
            }
            return temp;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static void MatrixAddColumns(double[,] A, double[,] B, int column)
        {
            for (int i = 0; i < A.GetLength(0); i++)
            {
                B[i, column] = A[i, column];
            }
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[] VectorDiff(double[] x, double[] y)
        {
            double[] temp = new double[x.Length];

            for (int i = 0; i < x.Length; i++)
            {
                temp[i] = x[i] - y[i];
            }

            return temp;
        }

        // --------------------------------------------------------------------------------------------------------------
        public static double[,] MatrixMultiply(double[,] A, double[,] B) 
        {
            int mA = A.GetLength(0);
            int nA = A.GetLength(1);
            int mB = B.GetLength(0);
            int nB = B.GetLength(1);
            if (nA != mB) 
            {
                Console.WriteLine("Illegal matrix dimensions.");
                return null;
            }
            double[,] C = new double[mA,nB];
            for (int i = 0; i < mA; i++)
                for (int j = 0; j < nB; j++)
                    for (int k = 0; k < nA; k++)
                    {
                        C[i, j] += (A[i, k] * B[k, j]);
                        
                    }
            return C;
        }

        public static double[,] MatrixInverse(double[,] matrix)
        {
            // use clever matrix decomposition technique.
            // returns null on error.
            int r = matrix.GetLength(0);
            int c = matrix.GetLength(1);
            if (r != c)
            {
                Console.WriteLine("Attempt to MatrixInverse a non-square mattrix");
                return null;
            }

            int n = r;
            double[,] result = MatrixCreate(n, n);
            double[] col = new double[n];
            double[] x = new double[n];

            int[] indx = new int[n];
            double d;
            double[,] luMatrix = MatrixDecomposition(matrix, indx, out d);

            if (luMatrix == null) return null;

            for (int j = 0; j < n; ++j)
            {
                for (int i = 0; i < n; ++i) { col[i] = 0.0; }
                col[j] = 1.0;
                x = MatrixBackSub(luMatrix, indx, col);
                for (int i = 0; i < n; ++i) { result[i,j] = x[i]; }
            }
            return result;
        } // MatrixInverse

        public static double[] MatrixBackSub(double[,] luMatrix, int[] indx, double[] b)
        {
            int rows = luMatrix.GetLength(0);
            int cols = luMatrix.GetLength(1);
            if (rows != cols)
            {
                Console.WriteLine("Non-square LU mattrix");
                return null;
            }

            int ii = 0; int ip = 0;
            int n = b.Length;
            double sum = 0.0;

            double[] x = new double[b.Length];
            b.CopyTo(x, 0);

            for (int i = 0; i < n; ++i)
            {
                ip = indx[i];
                sum = x[ip];
                x[ip] = x[i]; //
                if (ii == 0)
                {
                    for (int j = ii; j <= i - 1; ++j)
                    { 
                        sum -= luMatrix[i,j] * x[j]; 
                    }
                }
                else if (sum == 0.0)
                {
                    ii = i;
                }
                x[i] = sum;
            } // i

            for (int i = n - 1; i >= 0; --i)
            {
                sum = x[i];
                for (int j = i + 1; j < n; ++j)
                    { sum -= luMatrix[i,j] * x[j]; }
                x[i] = sum / luMatrix[i,i];
            }
              return x;
            } // MatrixBackSub

        public static double[,] MatrixDecomposition(double[,] matrix, int[] indx, out double d)
        {
            // see “Numerical Recipes in C”, 2nd ed., pp. 46-47
            // implements Crout’s method with partial pivoting

            int rows = matrix.GetLength(0);
            int cols = matrix.GetLength(1);
            if (rows != cols)
            {
                throw new Exception("Attempt to MatrixDecomposition a non-square mattrix");
            }

            int n = rows; // to sync with book notation
            int imax = 0; //
            double big = 0.0; double temp = 0.0; double sum = 0.0;
            double[] vv = new double[n];
            d = 1.0; // an out ref parameter

            double[,] result = MatrixDuplicate(matrix);
            // make a copy of the input array
 
            for (int i = 0; i < n; ++i)
            {
                big = 0.0;
                for (int j = 0; j < n; ++j)
                {
                    if (i == 72 && j == 72)
                    {
                        Console.WriteLine();
                    }
                    temp = Math.Abs(result[i,j]); // kind of wacky
                    if (temp > big)
                        big = temp;
                }
                if (big == 0.0)
                    return null; // consider throwing an Exception instead
                vv[i] = 1.0 / big;
            }

            for (int j = 0; j < n; ++j) // each column
            {
            for (int i = 0; i < j; ++i)
            {
                sum = result[i,j];
                for (int k = 0; k < i; ++k) { sum -= result[i,k] * result[k,j]; }
                result[i,j] = sum;
            } // i

            big = 0.0;
            for (int i = j; i < n; ++i)
            {
                sum = result[i,j];
                for (int k = 0; k < j; ++k) { sum -= result[i,k] * result[k,j]; }
                result[i,j] = sum;
                temp = vv[i] * Math.Abs(sum);
                if (temp >= big)
                {
                big = temp;
                imax = i;
                }
            } // i

            if (j != imax)
            {
                for (int k = 0; k < n; ++k)
                {
                temp = result[imax,k];
                result[imax,k] = result[j,k];
                result[j,k] = temp;
                }
                d = -d; // toggle the sign
                vv[imax] = vv[j];
            }
            indx[j] = imax;
            //Console.WriteLine(“Setting indx[" + j + "] to ” + imax);
            if (result[j,j] == 0.0)
                result[j,j] = 1.0e-20;

            if (j != n - 1)
            {
                temp = 1.0 / result[j,j];
                for (int i = j + 1; i < n; ++i) { result[i,j] *= temp; }
            }

            } // j (each column)

            return result;
        } // MatrixDecomposition

        public static int[] MatrixTwoToOneDim(int[,] A, int rows, int cols)
        {
            int[] temp = new int[rows * cols];
            int counter = 0;

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    temp[counter++] = A[i, j];
                }
            }

            return temp;
        }
        public static double[] MatrixTwoToOneDimDB(double[,] A, int rows, int cols)
        {
            double[] temp = new double[rows * cols];
            int counter = 0;

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    temp[counter++] = A[i, j];
                }
            }

            return temp;
        }
        public static double[,] VectorToTwoDim(double[] A)
        {
            double[,] x = new double[A.Length, 1];

            for (int i = 0; i < A.Length; i++)
            {
                x[i, 0] = A[i];
            }

            return x;
        }
    }
}


// moze kiedys do uzycia

/*
        private int _nrows;
        private int _ncols;
        private int[ , ] _data;

        public int nrows {
            get 
            { 
                return _nrows; 
            }
            set
            {

            }
        }
        public int ncols
        {
            get
            {
                return _ncols;
            }
            set
            {

            }
        }
        public int[,] data
        {
            get
            {
                return _data;
            }
            set
            {

            }
        }

        public Matrix()
        {
        }

        public Matrix(int[,] dat)
        {
            this._data = dat;
            this._nrows = dat.GetLength(0);
            this._ncols = dat.GetLength(1);
        }

        public Matrix(int nrow, int ncol) 
        {
            this._nrows = nrow;
            this._ncols = ncol;
            _data = new int[nrow,ncol];
        }

        public static Matrix transpose(Matrix matrix)
        {
            Matrix transposedMatrix = new Matrix(matrix.ncols, matrix.nrows);
            for (int i = 0; i < matrix.nrows; i++)
            {
                for (int j = 0; j < matrix.ncols; j++)
                {
                    transposedMatrix.setValueAt(j, i, matrix.getValueAt(i, j));
                }
            }
            return transposedMatrix;
        }

        public void setValueAt(int row, int col, int value)
        {
            _data[row, col] = value;
        }
        public int getValueAt(int row, int col)
        {
            return _data[row, col];
        }
        */


// moze kiedys do uzycia

/*
        private int _nrows;
        private int _ncols;
        private int[ , ] _data;

        public int nrows {
            get 
            { 
                return _nrows; 
            }
            set
            {

            }
        }
        public int ncols
        {
            get
            {
                return _ncols;
            }
            set
            {

            }
        }
        public int[,] data
        {
            get
            {
                return _data;
            }
            set
            {

            }
        }

        public Matrix()
        {
        }

        public Matrix(int[,] dat)
        {
            this._data = dat;
            this._nrows = dat.GetLength(0);
            this._ncols = dat.GetLength(1);
        }

        public Matrix(int nrow, int ncol) 
        {
            this._nrows = nrow;
            this._ncols = ncol;
            _data = new int[nrow,ncol];
        }

        public static Matrix transpose(Matrix matrix)
        {
            Matrix transposedMatrix = new Matrix(matrix.ncols, matrix.nrows);
            for (int i = 0; i < matrix.nrows; i++)
            {
                for (int j = 0; j < matrix.ncols; j++)
                {
                    transposedMatrix.setValueAt(j, i, matrix.getValueAt(i, j));
                }
            }
            return transposedMatrix;
        }

        public void setValueAt(int row, int col, int value)
        {
            _data[row, col] = value;
        }
        public int getValueAt(int row, int col)
        {
            return _data[row, col];
        }
        */